Поиск жёсткого преобразования облака точек

Ivan Ryzhikov

iwanryzij@yandex.ru

Mark Ilyasov

Постановка задачи

Даны два облака точек. Найти жёсткое преобразование из одного в другое

• Соответвие между начальными и конечными точками не известно

• Данные имеют шум

Note

Жёсткое преобразование сохраняет расстояние между точками, т.е. поворот + параллельный перенос

Более формально

Дано: \(X = \{ \mathbf{x}_1, \mathbf{x}_2, \dots, \mathbf{x}_N \} \subset \mathbb{R}^n\), \(Y = \{ \mathbf{y}_1, \mathbf{y}_2, \dots, \mathbf{y}_N \} \subset \mathbb{R}^n\)

\[ \mathbf{y}_{\pi(i)} = R \mathbf{x}_i + t + \boldsymbol{\epsilon}_i, \quad \forall i = 1, \dots, N, \]

где

• \(\boldsymbol{\epsilon}_i \in \mathbb{R}^n\) — вектор шума для точки \(i\), \(\boldsymbol{\epsilon}_i \sim \mathcal{N}(0,\, (\sigma^2, \sigma^2,\sigma^2))\)

• \(R \in SO(3)\) — матрица вращения \(R^T R = I\), \(\det(R) = 1\)

• \(t \in \mathbb{R}^n\) — вектор смещения,

• \(\pi: \{1, \dots, N\} \to \{1, \dots, N\}\) — перестановка индексов.

\[ \underset{R \in SO(3),\,t,\,\pi}{\arg \min}\ \sum_{i=1}^N \| R \mathbf{x}_i + t - \mathbf{y}_{\pi(i)} + \boldsymbol{\epsilon}_i \|^2, \]

В терминах матриц

\[ Y^T = R X^T P + t \begin{pmatrix} 1 & \dots & 1 \end{pmatrix}_{1 \times N} + E, \]

• \(\|\cdot\|_F\) — фробениусова норма,
• \(R \in SO(3)\) — матрица вращения (\(R^T R = I\), \(\det(R) = 1\)),
• \(P\) — матрица перестановки
  • \(P_{ij} \in \{0, 1\}\);
  • каждая строка и столбец имеют одну единицу — \(\forall i \sum_{j=1}^N P_{ij} = 1\), \(\forall j \sum_{i=1}^N P_{ij} = 1\)
• \(E \in \mathbb{R}^{n \times N}\) — матрица шума с \(E[:, i] = \boldsymbol{\epsilon}_i\).

\[ \underset{R \in SO(3),\,t,\,P}{\arg \min}\ \| R X^T P + t \begin{pmatrix} 1 & \dots & 1 \end{pmatrix}_{1 \times N} - Y^T \|_F^2 \]

Проблема

\[ \underset{R \in SO(3),\,t,\,P}{\arg \min}\ \| R X^TP + t \begin{pmatrix} 1 & . . . & 1 \end{pmatrix}_{1 \times N} - Y^T \|_F^2 \]

Ключевая сложность — поиск матрицы соответствия.

Целевая функция приобретает дискретную форму — обычные методы оптимизации не применимы

Алгоритм Кабша(-Умеямы) умеет решать успрощенную задачу с известным соответствием

\[ \underset{R \in SO(3),\,t}{\arg \min}\ \| R X^T + t \begin{pmatrix} 1 & \dots & 1 \end{pmatrix}_{1 \times N} - Y^T \|_F^2 \]

Решение

Избавимся от матрицы перестановок — будем уменьшать расстояние между облаками.

Изменим целевую функцию

\[ \underset{{R \in SO(3),\ t}}{\arg \min}\ \sum_{i=1}^N \| R \mathbf{x}_i + t - \mathbf{y}_{\sigma(i)} \|^2 \]

• \(\sigma(i)\) — индекс ближайшей точки \(\mathbf{y}_{\sigma(i)} \in Y\) для \(\mathbf{x}_i \in X\) ¹

1. отображение не биективно, индексы могут совпадать для разных точек

Идея алгоритма ICP

Iteartive Closest Point — итеративный метод поиска жёсткого преобразования через сопоставление ближайших точек. В качестве приближенного соответсвия точек используется сопоставление каждой ближайшей к ней (из другого облака). Для полученного соответствия можно использовать алгоритм Кабша

Этапы алгоритма ICP

1. Инициализация начального преобразования
2. Итерация цикла
   1. Применение текущего преобразования
   2. Построение соответствия между точками через поиск ближайшей
   3. Обновление приближения через алгоритм Кабша для найденного соответсвия
   4. Проверка сходимости алгоритма через метрику

Подготовка к исследованию методов

Генерация примеров

Исследование метода ICP

Ограниченность ICP

Использование упрощенной метрики ведет к тому, что целевая функция преобретает значительное количество локальных минимумов. В связи с чем, использование алгоритма ограничено: ICP показывает хорошие результаты только при малых углах поворота